/**
 * 相机相关的坐标系计算
 */
#include "opencv2/core.hpp"
#include <opencv2/core/utility.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/videoio.hpp"
#include "opencv2/highgui.hpp"

#include <iostream>

using namespace cv;
using namespace std;

/// 按照标定板上的数据，模拟数据
static void createSimuData(Mat &M, Mat& D,
                           vector<Point3f>& objectPoints,
                           vector<Point2f> &imagePoints
                           )
{
    Size boardSize = Size(11,9);             // 定标板上每行、列的角点数
    Size2d squareSize = Size2d(30,30);

    M = (cv::Mat_<double>(3, 3) << 4.4029373742487251e+03, 0., 2.0510437067385906e+03, 0.,
            4.4023532275269981e+03, 1.4869326229312501e+03, 0., 0., 1.);
    D = (cv::Mat_<double>(5, 1) <<  -1.0581567389661781e-01, 1.3360700226224864e-01,
         -4.2517045552156838e-04, -4.4498813903939513e-04, 0. );

    objectPoints.resize(0);
    for( int i = 0; i < boardSize.height; i++ )
        for( int j = 0; j < boardSize.width; j++ )
            objectPoints.push_back(Point3f(float(j*double(squareSize.width)),
                                      float(i*squareSize.height), 0));

    vector<double> data = { 2.24853906e+03, 2.30352637e+03, 2.19804443e+03, 2.30648438e+03,
            2.14649243e+03, 2.30949976e+03, 2.09387866e+03, 2.31252979e+03,
            2.04014648e+03, 2.31559717e+03, 1.98521216e+03, 2.31873877e+03,
            1.92917639e+03, 2.32188794e+03, 1.87188013e+03, 2.32510278e+03,
            1.81337549e+03, 2.32832935e+03, 1.75356873e+03, 2.33161914e+03,
            1.69245557e+03, 2.33496655e+03, 2.24596216e+03, 2.23455396e+03,
            2.19554419e+03, 2.23680981e+03, 2.14408594e+03, 2.23908008e+03,
            2.09153149e+03, 2.24140015e+03, 2.03786792e+03, 2.24372021e+03,
            1.98304626e+03, 2.24608081e+03, 1.92707947e+03, 2.24845825e+03,
            1.86989612e+03, 2.25086914e+03, 1.81146399e+03, 2.25329785e+03,
            1.75175916e+03, 2.25575317e+03, 1.69077576e+03, 2.25824048e+03,
            2.24340259e+03, 2.16584106e+03, 2.19306030e+03, 2.16738477e+03,
            2.14166602e+03, 2.16893066e+03, 2.08917920e+03, 2.17049194e+03,
            2.03560620e+03, 2.17208398e+03, 1.98082715e+03, 2.17371387e+03,
            1.92498694e+03, 2.17529565e+03, 1.86789404e+03, 2.17692065e+03,
            1.80956921e+03, 2.17854199e+03, 1.74998340e+03, 2.18016943e+03,
            1.68908984e+03, 2.18182422e+03, 2.24080396e+03, 2.09735742e+03,
            2.19055981e+03, 2.09820898e+03, 2.13923828e+03, 2.09904712e+03,
            2.08682715e+03, 2.09987622e+03, 2.03334131e+03, 2.10072290e+03,
            1.97871838e+03, 2.10155518e+03, 1.92290784e+03, 2.10241235e+03,
            1.86591321e+03, 2.10323535e+03, 1.80770044e+03, 2.10407495e+03,
            1.74820850e+03, 2.10486768e+03, 1.68744019e+03, 2.10571875e+03,
            2.23823242e+03, 2.02909778e+03, 2.18804126e+03, 2.02924963e+03,
            2.13676074e+03, 2.02940295e+03, 2.08450220e+03, 2.02950269e+03,
            2.03108557e+03, 2.02961755e+03, 1.97653137e+03, 2.02969714e+03,
            1.92082092e+03, 2.02977734e+03, 1.86393848e+03, 2.02983679e+03,
            1.80577856e+03, 2.02989026e+03, 1.74644617e+03, 2.02990698e+03,
            1.68580420e+03, 2.02991028e+03, 2.23567188e+03, 1.96112305e+03,
            2.18554590e+03, 1.96059241e+03, 2.13435693e+03, 1.95999365e+03,
            2.08214111e+03, 1.95938171e+03, 2.02881763e+03, 1.95875708e+03,
            1.97437085e+03, 1.95813000e+03, 1.91876709e+03, 1.95744226e+03,
            1.86198462e+03, 1.95672852e+03, 1.80397900e+03, 1.95599097e+03,
            1.74472473e+03, 1.95521765e+03, 1.68418079e+03, 1.95440442e+03,
            2.23307349e+03, 1.89337170e+03, 2.18303320e+03, 1.89216028e+03,
            2.13194775e+03, 1.89085889e+03, 2.07981177e+03, 1.88955017e+03,
            2.02655823e+03, 1.88820850e+03, 1.97216760e+03, 1.88683215e+03,
            1.91666077e+03, 1.88540967e+03, 1.86003857e+03, 1.88392468e+03,
            1.80214392e+03, 1.88240820e+03, 1.74301428e+03, 1.88081494e+03,
            1.68260803e+03, 1.87922400e+03, 2.23048340e+03, 1.82589197e+03,
            2.18050806e+03, 1.82399304e+03, 2.12952612e+03, 1.82201794e+03,
            2.07746021e+03, 1.81999634e+03, 2.02432263e+03, 1.81794104e+03,
            1.97006909e+03, 1.81581921e+03, 1.91467590e+03, 1.81364343e+03,
            1.85810339e+03, 1.81142322e+03, 1.80030933e+03, 1.80914368e+03,
            1.74131592e+03, 1.80679797e+03, 1.68101917e+03, 1.80436218e+03,
            2.22785791e+03, 1.75869739e+03, 2.17798120e+03, 1.75612378e+03,
            2.12708130e+03, 1.75345117e+03, 2.07512793e+03, 1.75071484e+03,
            2.02208997e+03, 1.74796436e+03, 1.96793018e+03, 1.74510840e+03,
            1.91263879e+03, 1.74221155e+03, 1.85618933e+03, 1.73924561e+03,
            1.79852295e+03, 1.73618835e+03, 1.73964038e+03, 1.73306897e+03,
            1.67949084e+03, 1.72986755e+03};
    for(int i=0; i< data.size()/2; i++) {
        imagePoints.push_back(Point2f(data[2*i], data[2*i+1]));
    }
}

/// 计算在相机空间的坐标
/// 输入: M, D, objectPoints(3D), imagePoints(2D)
/// 输出: 空间{c} 下的坐标
static void solvePnPDemo()
{
    // solvePnP -> R|t -> projectPoints -> projectedPoints
    Mat M, D;
    vector<Point3f>  objectPoints;
    vector<Point2f>  imagePoints;


    createSimuData(M, D, objectPoints, imagePoints);
    cout << "objectPoints: " << objectPoints << endl;
    cout << "imagePoints: " << imagePoints << endl;

    Mat rvec1, tvec1;
    solvePnP(objectPoints, imagePoints, M, D, rvec1, tvec1);

    cout << "R|t :" << rvec1.t() << ", " << tvec1.t() << endl;

    Mat R1;
    Rodrigues(rvec1, R1);
    cout << "Mat R:" << R1 << endl;
//    for (uint i=0; i< objectPoints.size(); i++) {
//        Mat p(3, 1, CV_64F);
//        p.at<double>(0,0) = objectPoints[i].x;
//        p.at<double>(1,0) = objectPoints[i].y;
//        p.at<double>(2,0) = objectPoints[i].z;
//        Mat p_c = R1*p + tvec1;
//        cout << i+1 << ": " << p_c.t() << endl;
//    }

    vector<Point2f> projectedPoints;
    projectPoints(objectPoints, rvec1, tvec1, M, D, projectedPoints);
    cout << "projectedPoints: " << projectedPoints << endl;
}

int main(int argc, char *argv[])
{
    int option = 1;
    switch (option) {
    case 1: // 3D - 2D 投影矩阵计算
        // TODO: 和我理解的不一样，后面好好理解
        solvePnPDemo();
        break;
    default:
        break;
    }

    return 0;
}
